import numpy as np

class CalcCost:
    def __init__(self, lateralTraj, longTraj):
        self.lateralTraj = lateralTraj
        self.longTraj = longTraj
        self.TrajSortByCost = []

    def run(self):
        for laterT in self.lateralTraj:
            for longT in self.longTraj:
                total_dis = 0
                s_list = np.arange(0, laterT["var"], 0.5)
                for s in s_list:
                    total_dis += laterT["f"](s)
                cost = total_dis / len(s_list)
                tTraj = [cost, laterT, longT]
                self.TrajSortByCost.append(tTraj)
        self.TrajSortByCost.sort(key=lambda x: (x[0]))

    def getTop(self):
        tTraj = self.TrajSortByCost[0]
        self.TrajSortByCost.pop(0)
        return tTraj